/**
 * Copyright (C) BFH www.bfh.ch 2011
 * Code written by: Patrick Dobler, Marc Folly
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
package ch.bfh.ti.kybernetik.simulator.controller.roboter;

import java.util.Collection;
import java.util.Collections;
import java.util.Iterator;
import java.util.LinkedList;
import java.util.List;

import org.apache.commons.collections.buffer.CircularFifoBuffer;

import ch.bfh.ti.kybernetik.simulator.controller.ThreadSafeIterationAction;
import ch.bfh.ti.kybernetik.simulator.model.Roboter;

public class RoboterController {

	private static final int MOTOR_MAX_SPEED = 500;

	private double roboterBottomLenght = 20;

	private final Roboter roboter;

	@SuppressWarnings("unchecked")
	private Collection<RoboterMoveState> roboterMoveStates = Collections.synchronizedCollection(new CircularFifoBuffer(500));

	public RoboterController(Roboter roboter) {
		this.roboter = roboter;
	}

	public void calculateNewRoboterPosition(int leftLightSensor, int rightLightSensor) {
		roboter.setLeftLightSensor(leftLightSensor);
		roboter.setRightLightSensor(rightLightSensor);
		// START TODO
		java.util.Random random = new java.util.Random();
		int leftSpeed = random.nextInt(25); //
		int rightSpeed = random.nextInt(25);
		boolean plus = random.nextBoolean();
		double leftMotorSpeed = roboter.getLeftMotor().getVelocity();
		double rightMotorSpeed = roboter.getRightMotor().getVelocity();
		double newLeftMotorSpeed = 0;
		double newRightMotorSpeed = 0;
		if (plus) {
			newLeftMotorSpeed = Math.abs(leftMotorSpeed + leftSpeed);
			newRightMotorSpeed = Math.abs(rightMotorSpeed + rightSpeed);
		} else {
			newLeftMotorSpeed = Math.abs(leftMotorSpeed - leftSpeed);
			newRightMotorSpeed = Math.abs(rightMotorSpeed - rightSpeed);
		}
		newLeftMotorSpeed %= MOTOR_MAX_SPEED;
		newRightMotorSpeed %= MOTOR_MAX_SPEED;

		roboter.getLeftMotor().setVelocity(newLeftMotorSpeed);
		roboter.getRightMotor().setVelocity(newRightMotorSpeed);
		// END TODO

		final RoboterMoveState currentRoboterMoveState = RoboterMoveStateFactory.getInstance().createRoboterMoveState(roboter,
				roboterBottomLenght);
		this.roboterMoveStates.add(currentRoboterMoveState);
		roboter.setX(currentRoboterMoveState.getNewRobotorPositionForwardX());
		roboter.setY(currentRoboterMoveState.getNewRobotorPositionForwardY());
		roboter.setDirection(currentRoboterMoveState.getNewDirectionVector());
	}

	public List<RoboterMoveState> calculateEstimatedRoboterMoveStates(int count) {
		Roboter iterationRoboter = new Roboter(this.roboter);
		List<RoboterMoveState> estimatedRoboterMoveStates = new LinkedList<RoboterMoveState>();
		for (int i = 0; i < count; i++) {
			RoboterMoveState roboterMoveState = RoboterMoveStateFactory.getInstance().createRoboterMoveState(iterationRoboter,
					roboterBottomLenght);
			estimatedRoboterMoveStates.add(roboterMoveState);
			// Set new roboter data
			iterationRoboter.setX(roboterMoveState.getNewRobotorPositionForwardX());
			iterationRoboter.setY(roboterMoveState.getNewRobotorPositionForwardY());
			iterationRoboter.setDirection(roboterMoveState.getNewDirectionVector());
		}
		return estimatedRoboterMoveStates;
	}

	public int getRightLightSensorXPos() {
		// TODO Auto-generated method stub
		return 0;
	}

	public int getRightLightSensorYPos() {
		// TODO Auto-generated method stub
		return 0;
	}

	public int getLeftLightSensorXPos() {
		return 0;
	}

	public int getLeftLightSensorYPos() {
		return 0;
	}

	public Roboter getRoboter() {
		return roboter;
	}

	public double getRoboterBottomLenght() {
		return roboterBottomLenght;
	}

	public void iterateRoboterMoveStates(ThreadSafeIterationAction<RoboterMoveState> action) {
		Collection<RoboterMoveState> list = getRoboterMoveStates();
		synchronized (list) {
			final Iterator<RoboterMoveState> i = list.iterator();
			while (i.hasNext()) {
				action.performAction(i.next());
			}
		}
	}

	private Collection<RoboterMoveState> getRoboterMoveStates() {
		return (roboterMoveStates);
	}

}
